package various;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.ColorSensor;

public class LabRead {
	private static int m; // righe
	private static int n; // colonne
	private static int[][] labirinto;

	private static int entrata; // righe
	private static int uscita; // colonne

	private static int angle_x; // movimenti dx-sx
	private static int angle_y;// movimenti su-giu
	private static int cicli_int;
	private static int cicli_ex;
	private static int speed;
	private static int speed_rw;

	public static void main(String[] args) throws Exception {

		m = 16;
		n = 12;
		cicli_ex = m;// righe
		cicli_int = n;// colonne

		labirinto = new int[m][n];

		angle_y = -30;
		angle_x = 230;

		speed = 75;
		speed_rw = 1000;

		Colori.initColori();
		ColorSensor cs = new ColorSensor(SensorPort.S1);

		calibra(cs);
		Thread.sleep(2000);

		// lettura labirinto

		for (int i = 0; i < cicli_ex; i++) {
			for (int j = 0; j < cicli_int; j++) {
				Motor.A.regulateSpeed(true);
				LCD.clear();
				LCD.drawString(Colori.getColore(cs.getColorNumber()) + "  i:"
						+ i + " j:" + j, 0, 1); // legge il colore
				labirinto[i][j] = cs.getColorNumber();
				if (i == 0 && labirinto[i][j] == 17)
					entrata = j;
				if (i == m - 1 && labirinto[i][j] == 17)
					uscita = j;
				Thread.sleep(500);
				Motor.A.rotate(angle_x);
				Motor.A.stop();
			}

			riavvolgiMotore(angle_x, cicli_int, MotorPort.A);
			Motor.B.regulateSpeed(true);
			Motor.B.setSpeed(speed);
			Motor.B.rotate(angle_y);
			Thread.sleep(500);
		}
	}

	private static void riavvolgiMotore(int angle, int cicli, MotorPort port) {
		Motor motore = new Motor(port);
		motore.setSpeed(speed_rw);
		motore.rotate(-angle * cicli);
		motore.setSpeed(speed);
	}

	public static void calibra(ColorSensor cs) throws Exception {
		Motor.A.regulateSpeed(true);
		int c = 1;
		int d = 1;
		while (c == 1 && d == 1) {
			LCD.clear();
			LCD.drawString("Calib. bianco...", 0, 2);
			Thread.sleep(500);
			c = cs.initWhiteBalance();
			LCD.drawString("Bianco eseguita", 0, 3);
			LCD.drawInt(c, 16, 3);
			Thread.sleep(500);

			Motor.A.rotate(angle_x);
			LCD.clear();
			LCD.drawString("Calib. nero...", 0, 2);
			Thread.sleep(500);
			d = cs.initBlackLevel();
			LCD.drawString("Nero eseguita", 0, 3);
			LCD.drawInt(d, 14, 3);
			Thread.sleep(500);
		}
		LCD.clear();
		Motor.A.regulateSpeed(true);
		Motor.A.rotate(-angle_x);
		Thread.sleep(500);
		Motor.B.setSpeed(speed);
		Motor.B.regulateSpeed(true);
		Motor.B.rotate(angle_y);
		Thread.sleep(500);

	}

}
